﻿// Copyright (C) 2009 - 2013 EvTech Project
//
// This program is free software: you can redistribute it and/or modify
// it under the +terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see http://www.gnu.org/licenses/. 

//<summary> 
//Application for creating algorithms to run on Lego NXT brick.
// Currently supports moving back/forward, turning and one action.
// 2009, Ernest Arendarenko, Honkalampi-säätiö.
// 
// Modifications, 
// 2009-12-30, Jussi Nuutinen
// 2010-01-28, Pekka Korhonen
// 2010-11-24, Pekka Korhonen Home and PgUp steering added
// 2012-02-27, Pekka Korhonen actions and différent robot types added
// 2012-05-25, Pekka Korhonen Support for joystick added, no need to GlovePie anymore (SlimDX)
// 2012-05-28, Pekka Korhonen key bindings for closing connection and reacquiring joystick
// 2012-11-19, Pekka Korhonen The UI language is changed in finnish
// 
// Requirements:
// - .NET;
// - libraries in app folder: {Bram.NxtSharp.dll, Bram.Utilities.dll, DragDropLib.dll, SlimDX.dll};
// - own images at app/img directory 
// {fwd.png,bkwd.png,left.png,right.png,kaynnista.png,tyhjenna.png,action.png}
// otherwise they will be created and saved at the img directory;
// - optional nxt4.exe.config file.
// </summary>

using System;
using System.IO;
using System.Drawing;
using System.Drawing.Imaging;
using System.Threading;
using System.Windows;
using System.Windows.Forms;
using Bram.NxtSharp;

using DragDropLib;
using ComIDataObject = System.Runtime.InteropServices.ComTypes.IDataObject;
using DataObject = System.Windows.Forms.DataObject;
using System.Collections.Generic;
using SlimDX.DirectInput;

namespace nxt4
{
    /// <summary>
    /// The class implementing the program functionality.
    /// </summary>
    public partial class Form1 : Form
    {
        private enum ROBOTMODE {DRIVER, ALLIGATOR, ROBOHUMAN};
        private ROBOTMODE robotMode;
        private int motorTurnDelay;
        private int lastDraggedOverPb = -1;
        private int motorSpeed = 75;
        private int joystickSensitivity = 50;
        private PictureBox pbJustDropped;
        private PictureBox pbCurrentDraggedItem;
        private Padding instructionsPadding;
        private Bitmap bmp, bmpDrag, bmpDragUp, bmpDragDown, bmpDragLeft, bmpDragRight, bmpUp, bmpDown;
        private Form frmSplashProcessing;
        private Thread executeAlgorithm, gameControllerThread;
        private Boolean robotIsWalking = false;
        private Boolean gameControllerThreadRunning = false;
        private Boolean interruptProcessingAlgorithm = false;
        private Joystick joystick;
        private JoystickState joystickState;
        private int prevJoystickX, prevJoystickY, prevJoystickAction;
        private Boolean keyUpPressed, keyDownPressed, keyLeftPressed, keyRightPressed;
        delegate void SetInfoTextCallback(string text);

        /// <summary>
        /// Initialize the controls and values.
        /// </summary>
        public Form1()
        {
            InitializeComponent();
            CreatePbBackgrounds();
            joystickState = new JoystickState();
            Application.ApplicationExit += new EventHandler(this.OnApplicationExit);

            instructionsPadding = new Padding(80, 0, 10, 0);
            tsTxtComPort.Text = nxtBrick.COMPortName;
            motorTurnDelay = Properties.Settings.Default.TurnDelay;
            tsTxtMotorTurn.Text = motorTurnDelay.ToString();
            joystickSensitivity = Properties.Settings.Default.JoystickSensitivity;
            tsTxtJoystickSensitivity.Text = joystickSensitivity.ToString();

            frmSplashProcessing = new Form();
            frmSplashProcessing.StartPosition = System.Windows.Forms.FormStartPosition.CenterScreen;
            frmSplashProcessing.ClientSize = new System.Drawing.Size(300, 30);
            frmSplashProcessing.FormBorderStyle = System.Windows.Forms.FormBorderStyle.None;
            frmSplashProcessing.TopMost = true;
            frmSplashProcessing.AutoSize = true;
            frmSplashProcessing.ShowInTaskbar = false;
            frmSplashProcessing.KeyPress += new KeyPressEventHandler(frmSplashProcessing_KeyPress);
            
            Label lblProcessing = new Label();
            lblProcessing.Font = new System.Drawing.Font("Verdana", 14.25F, ((System.Drawing.FontStyle)((System.Drawing.FontStyle.Bold | System.Drawing.FontStyle.Italic))), System.Drawing.GraphicsUnit.Point, ((byte)(0)));
            lblProcessing.Dock = DockStyle.Fill;
            lblProcessing.Text = "Käsittely...";
            lblProcessing.TextAlign = ContentAlignment.MiddleCenter;
            lblProcessing.BorderStyle = BorderStyle.FixedSingle;
            frmSplashProcessing.Controls.Add(lblProcessing);
            /*
            if (!Directory.Exists("img"))
            {
                Directory.CreateDirectory("img");
                MessageBox.Show("Created images directory");
            }

            LoadPbImage(pbForward1, "img/fwd.png");
            LoadPbImage(pbBackward1, "img/bkwd.png");
            LoadPbImage(pbLeft, "img/left.png");
            LoadPbImage(pbRight, "img/right.png");
            LoadPbImage(pbAction, "img/action.png");
            LoadPbImage(pbGo, "img/kaynnista.png");
            LoadPbImage(pbClear, "img/tyhjenna.png");
            pbGo.Left = pbGo.Width * 4;
            pbClear.Left = panExecControl.Width / 2 - pbClear.Width * 3;
             * */

            driverModeLabel.Text = Properties.Settings.Default.DriverMode;
            if (driverModeLabel.Text.Equals(driverToolStripMenuItem.Text))
            {
                robotMode = ROBOTMODE.DRIVER;
            }
            else if (driverModeLabel.Text.Equals(alligatorToolStripMenuItem.Text))
            {
                robotMode = ROBOTMODE.ALLIGATOR;
            }
            else if (driverModeLabel.Text.Equals(robohumanToolStripMenuItem.Text))
            {
                robotMode = ROBOTMODE.ROBOHUMAN;
            }
            else
            {
                robotMode = ROBOTMODE.DRIVER;
            }
            setDriverMode(robotMode);
            connectedLabel.Text = "Ei yhteyttä robottiin";
            keyUpPressed = false;
            keyDownPressed = false;
            keyLeftPressed = false;
            keyRightPressed = false;
        }

        void frmSplashProcessing_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == (char)Keys.Escape)
            {
                interruptProcessingAlgorithm = true;
            }
        }

        private void setInfoText(String text)
        {
            if (this.tsTxtInfo != null && this.tsTxtInfo.GetCurrentParent() != null)
            {
                if (this.tsTxtInfo.GetCurrentParent().InvokeRequired)
                {
                    SetInfoTextCallback d = new SetInfoTextCallback(setInfoText);
                    this.Invoke(d, new object[] { text });
                }
                else
                {
                    this.tsTxtInfo.Text = text;
                }
            }
        }

        private void gameControllerPoller()
        {
            gameControllerThreadRunning = true;
            Console.Out.WriteLine("gameControllerPoller start");
            while (gameControllerThreadRunning)
            {
                if (gameControllerThreadRunning
                    && joystick != null && this.Enabled)
                {
                    PollGameController();
                }
                Thread.Sleep(50);
            }
            Console.Out.WriteLine("gameControllerPoller abort");
            gameControllerThread.Abort();
        }
        
        private void PollGameController()
        {
            try
            {
                if (joystick.Acquire().IsFailure)
                {
                    return;
                }

                if (joystick.Poll().IsFailure)
                {
                    return;
                }
                joystickState = joystick.GetCurrentState();
                bool[] buttonStates = joystickState.GetButtons();
                int actionSpeed = 0;
                for (int i = 0; i < buttonStates.Length; i++)
                {
                    if (buttonStates[i] == true)
                    {
                        if (i % 2 == 0)
                        {
                            actionSpeed = motorSpeed;
                        }
                        else
                        {
                            actionSpeed = -motorSpeed;
                        }
                        break;
                    }
                }
                if (actionSpeed != 0 && this.nxtBrick.IsConnected)
                {
                    switch (robotMode)
                    {
                        case ROBOTMODE.ALLIGATOR:
                            Bite();
                            break;
                        case ROBOTMODE.DRIVER:
                        case ROBOTMODE.ROBOHUMAN:
                            turnMotor(3, actionSpeed);
                            break;
                    }
                }
                else if (actionSpeed != prevJoystickAction && this.nxtBrick.IsConnected)
                {
                    coastMotor(3);
                }
                int joystickX = joystickState.X / 10;
                int joystickY = joystickState.Y / 10;
                if (Math.Abs(joystickX) < joystickSensitivity)
                    joystickX = 0;
                if (Math.Abs(joystickY) < joystickSensitivity)
                    joystickY = 0;
                if (this.nxtBrick.IsConnected)
                {
                    if (joystickY != 0 || joystickX != 0)
                    {
                        steerRobot(joystickX, -joystickY);
                    }
                    else if (joystickX != prevJoystickX || joystickY != prevJoystickY)
                    {
                        coastMotor(1);
                        coastMotor(2);
                    }
                }
                prevJoystickX = joystickX;
                prevJoystickY = joystickY;
                prevJoystickAction = actionSpeed;
            }
            catch (Exception)
            {
                releaseJoystick();
            }
        }

        private void releaseJoystick()
        {
            if (joystick != null)
            {
                joystick.Unacquire();
                joystick.Dispose();
                joystick = null;
                Console.Out.WriteLine("joystick disconnected");
                setInfoText("Joystik ei löydy");
            }
        }

        private void steerRobot(int x, int y)
        {
            int leftMotorSpeed = 0;
            int rightMotorSpeed = 0;
            if (y < 0)
            {
                leftMotorSpeed = -x + y;
                rightMotorSpeed = x + y;
            }
            else
            {
                leftMotorSpeed = x + y;
                rightMotorSpeed = -x + y;
            }
            
            if (leftMotorSpeed > 100 || leftMotorSpeed < -100)
            {
                rightMotorSpeed = rightMotorSpeed * 100 / Math.Abs(leftMotorSpeed);
                leftMotorSpeed = leftMotorSpeed * 100 / Math.Abs(leftMotorSpeed);
            }
            if (rightMotorSpeed > 100 || rightMotorSpeed < -100)
            {
                leftMotorSpeed = leftMotorSpeed * 100 / Math.Abs(rightMotorSpeed);
                rightMotorSpeed = rightMotorSpeed * 100 / Math.Abs(rightMotorSpeed);
            }
             
            switch (robotMode)
            {
                case ROBOTMODE.ALLIGATOR:
                case ROBOTMODE.DRIVER:
                    turnMotor(1, rightMotorSpeed);
                    turnMotor(2, leftMotorSpeed);
                    break;
            }
        }

        /// <summary>
        /// Method for loading images into pictureboxes. If an image does not exist,
        /// it will be created, written with filename text and saved under img dir.
        /// </summary>
        /// <param name="pb">Picture box to be assigned with image</param>
        /// <param name="pngPath">An image path with corresponding png file</param>
        private void LoadPbImage(PictureBox pb, String pngPath)
        {
            if (!File.Exists(pngPath))
                try
                {
                    Bitmap bitmap = new Bitmap(bmp.Width, bmp.Height);
                    using (Graphics graphic = Graphics.FromImage(bitmap))
                    {
                     graphic.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
                     graphic.DrawString(Path.GetFileNameWithoutExtension(pngPath), 
                         new Font("Arial", 10, FontStyle.Regular),
                         SystemBrushes.InfoText, new PointF(1, 35));
                        bitmap.Save(pngPath, System.Drawing.Imaging.ImageFormat.Png);
                    }
                    
                    MessageBox.Show("Graphics file was not found.\n\nCreated an empty image\n" + 
                        pngPath + "\n\nReplace it with your own image.",
                    "Missing file", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                    
                    pb.Image = System.Drawing.Bitmap.FromFile(pngPath);
                }
                catch (Exception ex)
                {
                    MessageBox.Show("There was a problem opening and saving the image\n" +
                        pngPath + "\n\n" + ex.ToString());
                }
            else 
                pb.Image = System.Drawing.Bitmap.FromFile(pngPath);
        }
        
        /// <summary>
        /// Create highlight(bmp) and placement mark(the rest bmp*) bitmaps
        /// </summary>
        private void CreatePbBackgrounds()
        {
            int Width = pbRight.Width;
            int Height = pbRight.Height;
            bmp = new Bitmap(Width, Height);
            bmpDragUp = new Bitmap(bmp);
            bmpDragDown = new Bitmap(bmp);
            bmpDragLeft = new Bitmap(bmp);
            bmpDragRight = new Bitmap(bmp);

            Graphics g = Graphics.FromImage(bmp);
            System.Drawing.Drawing2D.LinearGradientBrush lgb = new System.Drawing.Drawing2D.LinearGradientBrush(new Point(0, 0), new Point(Width, Height), Color.LightSkyBlue, Color.DeepSkyBlue);
            g.FillRectangle(lgb, 0, 0, Width, Height);

            System.Drawing.SolidBrush myBrush = new System.Drawing.SolidBrush(System.Drawing.Color.Navy);
            g = Graphics.FromImage(bmpDragDown);
            g.FillRectangle(myBrush, 0, Height - 5, Width, 5);
            g = Graphics.FromImage(bmpDragUp);
            g.FillRectangle(myBrush, 0, 0, Width, 5);
            g = Graphics.FromImage(bmpDragLeft);
            g.FillRectangle(myBrush, 0, 0, 5, Height);
            g = Graphics.FromImage(bmpDragRight);
            g.FillRectangle(myBrush, Width - 5, 0, Width, Height);

            bmpDown = bmpDragDown;
            bmpUp = bmpDragUp;
            //clean up
            myBrush.Dispose();
            lgb.Dispose();
            g.Dispose();
        }
        
        /// <summary>
        /// Start the algorithm in own thread with press on the execute button. 
        /// Disable the main form and show 'Processing' banner.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void pbGo_Click(object sender, EventArgs e)
        {
            this.Enabled = false;
            frmSplashProcessing.Show();
            executeAlgorithm = new Thread(Start);
            executeAlgorithm.Start();
        }

        /// <summary>
        /// Inits NSX connextion
        /// </summary>
        private void ConnectToNSX()
        { 
            //Connect to the available NXT brick
            try
            {
                nxtBrick.COMPortName = tsTxtComPort.Text;
                nxtBrick.Connect();
                Console.Write(">>connected");
            }
            catch (NxtChannelBusyException ex)
            {
                MessageBox.Show("NXT robotti on varattu:\n\n" + ex.ToString(),
                "Connection Failure", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                nxtBrick.Disconnect();
            }
            catch (Exception ex)
            {
                MessageBox.Show("NXT robottiin ei saada yhteyttä:\n\n" + ex.ToString(),
                "Connection Failure", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                nxtBrick.Disconnect();
            }
        }

        /// <summary>
        /// KeyPress
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void Form1_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == (char)Keys.Escape)
            {
                if (this.nxtBrick.IsConnected)
                {
                    coastMotor(1);
                    coastMotor(2);
                    coastMotor(3);
                    nxtBrick.Disconnect();
                }
            }
            if (e.KeyChar == 'j')
            {
                keyUpPressed = false;
                keyDownPressed = false;
                keyLeftPressed = false;
                keyRightPressed = false;
                setJoystick();
                
                if (this.nxtBrick.IsConnected && this.Enabled)
                {
                    coastMotor(1);
                    coastMotor(2);
                    coastMotor(3);
                }
            }
        }

        /// <summary>
        /// After the algorithm is done, return to algorithm edit mode.
        /// </summary>
        private void WakeApp()
        {
            Invoke((MethodInvoker)delegate()
            {
                this.Enabled = true;
                interruptProcessingAlgorithm = false;
                frmSplashProcessing.Hide();
            });
        }




        /// <summary>
        /// A key method to run the algorithm. Connects to a brick if it is not
        /// connected and saves the properties set in the menu. Then goes through
        /// all the pictureboxes in the algorithm panel and calls the respective
        /// action method.
        /// </summary>
        private void Start()
        {
            if (!nxtBrick.IsConnected & flpProgram.Controls.Count != 0) {
                //Connect to the available NXT brick
                ConnectToNSX();
            }


            //Execute the algorithm
            if (nxtBrick.IsConnected)
            {
                coastMotor(1);
                coastMotor(2);
                coastMotor(3);
                foreach (PictureBox pbt in flpProgram.Controls)
                {
                    switch (robotMode)
                    {
                        case ROBOTMODE.ROBOHUMAN:
                            if (pbt.Name.Equals("pbForward1"))
                            {
                                SetRobotSteady();
                                RotateMotor(1, 180);
                                RotateMotors(1800, 1800, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbBackward1"))
                            {
                                SetRobotSteady();
                                RotateMotor(1, -180);
                                RotateMotors(-1800, -1800, motorSpeed / 2);
                            }
                            else if (pbt.Name.Equals("pbRight"))
                            {
                                SetRobotSteady();
                                RotateMotor(2, 180);
                                RotateMotors(4320, 0, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbLeft"))
                            {
                                SetRobotSteady();
                                RotateMotor(1, 180);
                                RotateMotors(0, 4320, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbAction"))
                            {
                                RotateMotor(3, 180);
                            }
                            break;
                        case ROBOTMODE.DRIVER:
                            if (pbt.Name.Equals("pbForward1"))
                            {
                                RotateMotors(720, 720, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbBackward1"))
                            {
                                RotateMotors(-720, -720, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbRight"))
                            {
                                RotateMotors(-360, 360, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbLeft"))
                            {
                                RotateMotors(360, -360, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbAction"))
                            {
                                RotateMotor(3, 900);
                            }
                            break;
                        case ROBOTMODE.ALLIGATOR:
                            if (pbt.Name.Equals("pbForward1"))
                            {
                                RotateMotors(1800, 1800, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbBackward1"))
                            {
                                RotateMotors(-1800, -1800, motorSpeed);
                            }
                            else if (pbt.Name.Equals("pbRight"))
                            {
                                RotateMotor(2, 720);
                            }
                            else if (pbt.Name.Equals("pbLeft"))
                            {
                                RotateMotor(1, 720);
                            }
                            else if (pbt.Name.Equals("pbAction"))
                            {
                                Bite();
                            }
                            break;
                    }
                    Console.Write(">>" + pbt.Name);
                    if (interruptProcessingAlgorithm)
                    {
                        Console.WriteLine(" >>Interrupted<<");
                        break;
                    }
                }
                coastMotor(1);
                coastMotor(2);
                coastMotor(3);
            }
            WakeApp();
            Console.WriteLine(" >>Finished<<");
            executeAlgorithm.Abort();
        }

        #region Nxt Motor Management
        /// <summary>
        /// Rotates a specified NXT Brick motor.
        /// </summary>
        /// <param name="no">Motor index</param>
        /// <param name="value">Rotation speed</param>
        private void turnMotor(int no, int value)
        {
            if (no == 1)
            {
                nxtMotor1.Turn(value, 0);
               
            }
            else if (no == 2)
            {
                nxtMotor2.Turn(value, 0);
            }
            else if (no == 3)
            {
                nxtMotor3.Turn(value, 0);
            }
        }
        
        /// <summary>
        /// Stops rotating a motor
        /// </summary>
        /// <param name="no">Motor index</param>
        private void coastMotor(int no)
        {
            if (no == 1)
            {
                nxtMotor1.Coast();
            }
            else if (no == 2)
            {
                nxtMotor2.Coast();
            }
            else if (no == 3)
            {
                nxtMotor3.Coast();
            }
        }

        /// <summary>
        /// Brakes rotating a motor
        /// </summary>
        /// <param name="no">Motor index</param>
        private void brakeMotor(int no)
        {
            if (no == 1)
            {
                nxtMotor1.Brake();
            }
            else if (no == 2)
            {
                nxtMotor2.Brake();
            }
            else if (no == 3)
            {
                nxtMotor3.Brake();
            }
        }

        /// <summary>
        /// Listen keys so that the robot can be also driven with keyboard
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void Form1_KeyDown(object sender, KeyEventArgs e)
        {
            if (!this.nxtBrick.IsConnected)
            {
                //Connect to the available NXT brick
                ConnectToNSX();
                keyUpPressed = false;
                keyDownPressed = false;
                keyLeftPressed = false;
                keyRightPressed = false;
                return;
            }
            if (this.nxtBrick.IsConnected && this.Enabled)
            {

                if (robotMode == ROBOTMODE.ROBOHUMAN)
                {
                    switch (e.KeyCode)
                    {
                        case Keys.Up:
                            if (robotIsWalking)
                                break;
                            SetRobotSteady();
                            RotateMotor(1, 180);
                            turnMotor(1, motorSpeed);
                            turnMotor(2, motorSpeed);
                            break;
                        case Keys.Down:
                            if (robotIsWalking)
                                break;
                            SetRobotSteady();
                            RotateMotor(1, -180);
                            turnMotor(1, -motorSpeed/2);
                            turnMotor(2, -motorSpeed/2);
                            break;
                        case Keys.Left:
                            if (robotIsWalking)
                                break;
                            SetRobotSteady();
                            RotateMotor(1, 180);
                            turnMotor(2, motorSpeed);
                            break;
                        case Keys.Right:
                            if (robotIsWalking)
                                break;
                            SetRobotSteady();
                            RotateMotor(2, 180);
                            turnMotor(1, motorSpeed);
                            break;
                        case Keys.Home:
                            if (robotIsWalking)
                                break;
                            SetRobotSteady();
                            RotateMotor(1, 180);
                            turnMotor(2, motorSpeed);
                            break;
                        case Keys.PageUp:
                            if (robotIsWalking)
                                break;
                            SetRobotSteady();
                            RotateMotor(2, 180);
                            turnMotor(1, motorSpeed);
                            break;
                        case Keys.Space:
                            turnMotor(3, motorSpeed);
                            break;
                    }
                }
                else
                {
                    switch (e.KeyCode)
                    {
                        case Keys.Up:
                            keyUpPressed = true;
                            break;
                        case Keys.Down:
                            keyDownPressed = true;
                            break;
                        case Keys.Left:
                            keyLeftPressed = true;
                            break;
                        case Keys.Right:
                            keyRightPressed = true;
                            break;
                        case Keys.Space:
                        case Keys.PageDown:
                            switch (robotMode)
                            {
                                case ROBOTMODE.ALLIGATOR:
                                    Bite();
                                    break;
                                case ROBOTMODE.DRIVER:
                                    turnMotor(3, motorSpeed);
                                    break;
                            }
                            break;
                        case Keys.End:
                            switch (robotMode)
                            {
                                case ROBOTMODE.ALLIGATOR:
                                    Bite();
                                    break;
                                case ROBOTMODE.DRIVER:
                                    turnMotor(3, -motorSpeed);
                                    break;
                            }
                            break;
                    }
                    CheckArrowsKeys();
                }

            }
            else
            {
                Console.Out.WriteLine("Can't");
                connectedLabel.Text = "Yhteys meni poikki";
            }
        }
        private void CheckArrowsKeys()
        {
            int keyboardXSpeed = 0;
            int keyboardYSpeed = 0;

            if (keyUpPressed)
            {
                keyboardYSpeed += motorSpeed;
            }
            if (keyDownPressed)
            {
                keyboardYSpeed -= motorSpeed;
            }
            if (keyRightPressed)
            {
                keyboardXSpeed += motorSpeed;
            }
            if (keyLeftPressed)
            {
                keyboardXSpeed -= motorSpeed;
            }
            if (keyboardXSpeed == 0 && keyboardYSpeed == 0)
            {
                coastMotor(1);
                coastMotor(2);
            }
            else
            {
                steerRobot(keyboardXSpeed, keyboardYSpeed);
            }
        }

        private void SetRobotSteady()
        {
            robotIsWalking = true;
            turnMotor(1, motorSpeed);
            nxtPressureSensor1.Poll();
            while (nxtBrick.IsConnected && !nxtPressureSensor1.IsPressed)
            {
                Thread.Sleep(10);
                nxtPressureSensor1.Poll();
            }
            brakeMotor(1);
            turnMotor(2, motorSpeed);
            nxtPressureSensor2.Poll();
            while (nxtBrick.IsConnected && !nxtPressureSensor2.IsPressed)
            {
                Thread.Sleep(10);
                nxtPressureSensor2.Poll();
            }
            brakeMotor(2);
        }

        /// <summary>
        /// Listen keys so that the robot can be also driven with keyboard.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void Form1_KeyUp(object sender, KeyEventArgs e)
        {
            if (this.nxtBrick.IsConnected && this.Enabled)
            {
                switch (e.KeyCode)
                {
                    case Keys.Space:
                    case Keys.End:
                    case Keys.PageDown:
                        switch (robotMode)
                        {
                            case ROBOTMODE.ALLIGATOR:
                                break;
                            case ROBOTMODE.DRIVER:
                                coastMotor(3);
                                break;
                            case ROBOTMODE.ROBOHUMAN:
                                coastMotor(3);
                                break;
                        }
                        break;
                    case Keys.Up:
                        keyUpPressed = false;
                        break;
                    case Keys.Down:
                        keyDownPressed = false;
                        break;
                    case Keys.Right:
                        keyRightPressed = false;
                        break;
                    case Keys.Left:
                        keyLeftPressed = false;
                        break;
                }
                switch (robotMode)
                {
                    case ROBOTMODE.ALLIGATOR:
                    case ROBOTMODE.DRIVER:
                        CheckArrowsKeys();
                        break;
                    case ROBOTMODE.ROBOHUMAN:
                        if (!keyUpPressed && !keyDownPressed && !keyRightPressed && !keyLeftPressed)
                        {
                            robotIsWalking = false;
                            coastMotor(1);
                            coastMotor(2);
                        }
                        break;
                }
            } else {
                Console.Out.WriteLine("Can't");
            }

        }

        private void Bite()
        {
            turnMotor(1, motorSpeed);
            turnMotor(2, motorSpeed);
            RotateMotor(3, -360);
            Thread.Sleep(400);
            turnMotor(1, -motorSpeed);
            turnMotor(2, -motorSpeed);
            RotateMotor(3, 360);
            Thread.Sleep(600);
            coastMotor(1);
            coastMotor(2);
            coastMotor(3);
        }


        private void RotateMotors(int angle1, int angle2, int speed)
        {
            NxtGetOutputState Gos1, Gos2;

            nxtMotor1.ResetPosition(true);
            nxtMotor2.ResetPosition(true);

            if(angle1 != 0)
                turnMotor(1, (int)(System.Math.Sign(angle1) * speed));
            if (angle2 != 0)
                turnMotor(2, (int)(System.Math.Sign(angle2) * speed));

            Gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
            Gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);

            while (((Gos1.BlockTachoCount < angle1 && angle1 > 0 &&
                ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN))) ||
                (Gos1.BlockTachoCount > -angle1 && angle1 > 0 &&
                (robotMode == ROBOTMODE.ALLIGATOR)) ||
                ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN) && angle1 < 0 &&
                Gos1.BlockTachoCount > angle1) ||
                (Gos1.BlockTachoCount < -angle1 && angle1 < 0 &&
                (robotMode == ROBOTMODE.ALLIGATOR))
                ||
                (Gos2.BlockTachoCount < angle2 && angle2 > 0 &&
                ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN))) ||
                (Gos2.BlockTachoCount > -angle2 && angle2 > 0 &&
                (robotMode == ROBOTMODE.ALLIGATOR)) ||
                ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN) && angle2 < 0 &&
                Gos2.BlockTachoCount > angle2) ||
                (Gos2.BlockTachoCount < -angle2 && angle2 < 0 &&
                (robotMode == ROBOTMODE.ALLIGATOR)))
                && nxtBrick.IsConnected)
            {
                if((Gos1.BlockTachoCount >= angle1 && angle1 > 0 &&
                    ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN))) ||
                    (Gos1.BlockTachoCount <= -angle1 && angle1 > 0 &&
                    (robotMode == ROBOTMODE.ALLIGATOR)) ||
                    ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN) && angle1 < 0 &&
                    Gos1.BlockTachoCount <= angle1) ||
                    (Gos1.BlockTachoCount >= -angle1 && angle1 < 0 &&
                    (robotMode == ROBOTMODE.ALLIGATOR)))
                {
                    coastMotor(1);
                }
                if ((Gos2.BlockTachoCount >= angle2 && angle2 > 0 &&
                    ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN))) ||
                    (Gos2.BlockTachoCount <= -angle2 && angle2 > 0 &&
                    (robotMode == ROBOTMODE.ALLIGATOR)) ||
                    ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN) && angle2 < 0 &&
                    Gos2.BlockTachoCount <= angle2) ||
                    (Gos2.BlockTachoCount >= -angle2 && angle2 < 0 &&
                    (robotMode == ROBOTMODE.ALLIGATOR)))
                {
                    coastMotor(2);
                }
                Thread.Sleep(50);
                Gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
                Gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
            }
            brakeMotor(1);
            brakeMotor(2);
        }

        private void RotateMotor(int motor, int angle)
        {
            NxtGetOutputState currentGos;
            NxtMotor currentMotor;
            int direction = 0;
            if (motor == 1)
            {
                currentMotor = nxtMotor1;
                currentGos = nxtBrick.Comm.GetOutputState(currentMotor.Port);
            }
            else if (motor == 2)
            {
                currentMotor = nxtMotor2;
                currentGos = nxtBrick.Comm.GetOutputState(currentMotor.Port);
            }
            else
            {
                currentMotor = nxtMotor3;
                currentGos = nxtBrick.Comm.GetOutputState(currentMotor.Port);
            }
            if (angle < 0)
            {
                direction = -1;
            }
            else
            {
                direction = 1;
            }
            currentMotor.ResetPosition(true);
            turnMotor(motor, (int)(direction * motorSpeed));
            currentGos = nxtBrick.Comm.GetOutputState(currentMotor.Port);
            while ((currentGos.BlockTachoCount <= angle && direction >= 0 &&
                ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN))) ||
                (currentGos.BlockTachoCount >= -angle && direction >= 0 &&
                (robotMode == ROBOTMODE.ALLIGATOR))  ||
                ((robotMode == ROBOTMODE.DRIVER || robotMode == ROBOTMODE.ROBOHUMAN) && direction < 0 &&
                currentGos.BlockTachoCount >= angle) ||
                (currentGos.BlockTachoCount <= -angle && direction < 0 &&
                (robotMode == ROBOTMODE.ALLIGATOR))
                && nxtBrick.IsConnected)
            {
                Thread.Sleep(50);
                currentGos = nxtBrick.Comm.GetOutputState(currentMotor.Port);
            }
            brakeMotor(motor);
        }


        /// <summary>
        /// Rotates both motors forward for a specified period of time
        /// </summary>
        /// <param name="delay">Rotation time (ms)</param>
        private void MoveForward1(int delay)
        {
            NxtGetOutputState gos1, gos2;
            nxtMotor1.ResetPosition(true);
            nxtMotor2.ResetPosition(true);
            turnMotor(1, motorSpeed);
            turnMotor(2, motorSpeed);
            gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
            gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
            while ((gos1.BlockTachoCount < delay && (robotMode == ROBOTMODE.DRIVER)) ||
                (gos2.BlockTachoCount < delay && (robotMode == ROBOTMODE.DRIVER)) ||
                (gos1.BlockTachoCount > -delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                (gos2.BlockTachoCount > -delay && robotMode == ROBOTMODE.ALLIGATOR)
                && nxtBrick.IsConnected)
            {
                if ((gos1.BlockTachoCount >= delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos1.BlockTachoCount <= -delay && robotMode == ROBOTMODE.ALLIGATOR))
                {
                    brakeMotor(1);
                }
                if ((gos2.BlockTachoCount >= delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos2.BlockTachoCount <= -delay && robotMode == ROBOTMODE.ALLIGATOR))
                {
                    brakeMotor(2);
                }
                Thread.Sleep(100);
                gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
                gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
            }
            brakeMotor(1);
            brakeMotor(2);
        }
        /// <summary>
        /// Rotates both motors backward for a specified period of time
        /// </summary>
        /// <param name="delay">Rotation time (ms)</param>
        private void MoveBackward1(int delay)
        {
            NxtGetOutputState gos1, gos2;
            nxtMotor1.ResetPosition(true);
            nxtMotor2.ResetPosition(true);
            turnMotor(1, -motorSpeed);
            turnMotor(2, -motorSpeed);
            gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
            gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
            while ((gos1.BlockTachoCount < delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos2.BlockTachoCount < delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos1.BlockTachoCount > -delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos2.BlockTachoCount > -delay && (robotMode == ROBOTMODE.DRIVER))
                    && nxtBrick.IsConnected)
            {
                Thread.Sleep(100);
                gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
                gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
                if ((gos1.BlockTachoCount >= delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos1.BlockTachoCount <= -delay && (robotMode == ROBOTMODE.DRIVER)))
                {
                    brakeMotor(1);
                }
                if ((gos2.BlockTachoCount >= delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos2.BlockTachoCount <= -delay && (robotMode == ROBOTMODE.DRIVER)))
                {
                    brakeMotor(2);
                }
            }
            //Thread.Sleep(delay);
            //coastMotor(1);
            //coastMotor(2);
        }

        /// <summary>
        /// Rotates the first motor forward for a specified period of time
        /// </summary>
        /// <param name="delay">Rotation time (ms)</param>
        private void TurnLeft(int delay)
        {
            NxtGetOutputState gos1, gos2;
            nxtMotor1.ResetPosition(true);
            nxtMotor2.ResetPosition(true);
            turnMotor(1, motorSpeed);
            turnMotor(2, -motorSpeed);  // jussian
            gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
            gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
            while ((gos1.BlockTachoCount < delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos2.BlockTachoCount > -delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos1.BlockTachoCount > -delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos2.BlockTachoCount < delay && robotMode == ROBOTMODE.ALLIGATOR)
                    && nxtBrick.IsConnected)
            {
                Thread.Sleep(100);
                gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
                gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
                if ((gos1.BlockTachoCount >= delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos1.BlockTachoCount <= -delay && robotMode == ROBOTMODE.ALLIGATOR))
                {
                    brakeMotor(1);
                }
                if ((gos2.BlockTachoCount >= delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos2.BlockTachoCount <= -delay && (robotMode == ROBOTMODE.DRIVER)))
                {
                    brakeMotor(2);
                }
            }
            //Thread.Sleep(delay);
            //coastMotor(1);
            //coastMotor(2);  // jussian
        }
        
        /// <summary>
        /// Rotates the second motor forward for a specified period of time
        /// </summary>
        /// <param name="delay">Rotation time (ms)</param>
        private void TurnRight(int delay)
        {
            NxtGetOutputState gos1, gos2;
            nxtMotor1.ResetPosition(true);
            nxtMotor2.ResetPosition(true);
            turnMotor(2, motorSpeed);
            turnMotor(1, -motorSpeed); //jussian
            gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
            gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
            while ((gos1.BlockTachoCount > -delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos2.BlockTachoCount < delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos1.BlockTachoCount < delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos2.BlockTachoCount > -delay && robotMode == ROBOTMODE.ALLIGATOR)
                    && nxtBrick.IsConnected)
            {
                Thread.Sleep(100);
                gos1 = nxtBrick.Comm.GetOutputState(nxtMotor1.Port);
                gos2 = nxtBrick.Comm.GetOutputState(nxtMotor2.Port);
                if ((gos1.BlockTachoCount >= delay && robotMode == ROBOTMODE.ALLIGATOR) ||
                    (gos1.BlockTachoCount <= -delay && (robotMode == ROBOTMODE.DRIVER)))
                {
                    brakeMotor(1);
                }
                if ((gos2.BlockTachoCount >= delay && (robotMode == ROBOTMODE.DRIVER)) ||
                    (gos2.BlockTachoCount <= -delay && robotMode == ROBOTMODE.ALLIGATOR))
                {
                    brakeMotor(2);
                }
            }
            //Thread.Sleep(delay);
            //coastMotor(2);
            //coastMotor(1); //jussian
        }

        #endregion

        #region Event Handlers, D&D

        /// <summary>
        /// Highlight the instruction under the cursor
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void pb_MouseEnter(object sender, EventArgs e)
        {
            PictureBox pb = sender as PictureBox;
            pb.BackgroundImage = bmp;
        }

        /// <summary>
        /// Clear the instruction background when mouse leaves
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void pb_MouseLeave(object sender, EventArgs e)
        {
            PictureBox pb = sender as PictureBox;
            pb.BackgroundImage = null;
        }

        /// <summary>
        /// Clear the algorithm panel
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void pbClr_Click(object sender, EventArgs e)
        {
            flpProgram.Controls.Clear();
        }

        private void tsBtnExit_Click(object sender, EventArgs e)
        {
            Application.Exit();
        }

        //private void nxtSonar_ValueChanged(NxtSensor sensor)
        //{
        //    Invoke((MethodInvoker)delegate()
        //    {
        //        lblUltrasonic.Text = "Ultrasonic: " + nxtSonar.RawValue.ToString();
        //    });
        //}

        /*
        private void nxtPressureSensor_ValueChanged(NxtSensor sensor)
        {
            Invoke((MethodInvoker)delegate()
            {
                if (robotDirection == 1) //forward
                {
                    if (sensor == nxtPressureSensor1 && nxtPressureSensor1.IsPressed && robotMode == ROBOTMODE.ROBOHUMAN)
                    {
                        debugLabel.Text = "PressureSensor1 pressed";
                        brakeMotor(1);
                        turnMotor(2, (int)(motorBaseSpeed * motorSpeedFactor));
                    }
                    if (sensor == nxtPressureSensor2 && nxtPressureSensor2.IsPressed && robotMode == ROBOTMODE.ROBOHUMAN)
                    {
                        debugLabel.Text = "PressureSensor2 pressed";
                        brakeMotor(2);
                        turnMotor(1, (int)(motorBaseSpeed * motorSpeedFactor));
                    }
                }
                else if (robotDirection == -1) // backward
                {
                    if (sensor == nxtPressureSensor1 && nxtPressureSensor1.IsPressed && robotMode == ROBOTMODE.ROBOHUMAN)
                    {
                        debugLabel.Text = "PressureSensor1 pressed";
                        brakeMotor(1);
                        turnMotor(2, (int)(-motorBaseSpeed * motorSpeedFactor));
                    }
                    if (sensor == nxtPressureSensor2 && nxtPressureSensor2.IsPressed && robotMode == ROBOTMODE.ROBOHUMAN)
                    {
                        debugLabel.Text = "PressureSensor2 pressed";
                        brakeMotor(2);
                        turnMotor(1, (int)(-motorBaseSpeed * motorSpeedFactor));
                    }
                }
            });
        }
         */

        /// <summary>
        /// Start drag and drop process when dragging an instruction, show drag icon
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void pb_MouseDown(object sender, MouseEventArgs e)
        {
            PictureBox pbs = sender as PictureBox;

            pbCurrentDraggedItem = new PictureBox();
            pbCurrentDraggedItem.Size = new System.Drawing.Size(80, 80);
            pbCurrentDraggedItem.Padding = new Padding(10);
            pbCurrentDraggedItem.SizeMode = System.Windows.Forms.PictureBoxSizeMode.StretchImage;
            pbCurrentDraggedItem.TabStop = false;
            pbCurrentDraggedItem.Image = pbs.Image;
            pbCurrentDraggedItem.Name = pbs.Name;

            Bitmap bmp = (Bitmap)pbs.Image;
            DataObject data = new DataObject(new DragDropLib.DataObject());
            ShDragImage shdi = new ShDragImage();
            Win32Size size;
            size.cx = bmp.Width;
            size.cy = bmp.Height;
            shdi.sizeDragImage = size;
            Point p = e.Location;
            Win32Point wpt;
            wpt.x = p.X;
            wpt.y = p.Y;
            shdi.ptOffset = wpt;
            shdi.hbmpDragImage = bmp.GetHbitmap();
            shdi.crColorKey = Color.Magenta.ToArgb();
            IDragSourceHelper sourceHelper = (IDragSourceHelper)new DragDropHelper();
            sourceHelper.InitializeFromBitmap(ref shdi, data);
            DoDragDrop(data, DragDropEffects.Copy);
        }

        /// <summary>
        /// Drop action with two scenarios. 1 - an instruction is dragged from
        /// instructions panel. In this case it is then dropped to exec panel.
        /// 2 - an insr. was dragged inside exec panel. Just reorder it 
        /// to a new position
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void flpProgram_DragDrop(object sender, DragEventArgs e)
        {
            PictureBox pbs = pbCurrentDraggedItem;
            pbCurrentDraggedItem.BorderStyle = BorderStyle.None;
            e.Effect = DragDropEffects.Copy;
            Point p = Cursor.Position;
            Win32Point wp;
            wp.x = p.X;
            wp.y = p.Y;
            IDropTargetHelper dropHelper = (IDropTargetHelper)new DragDropHelper();
            dropHelper.Drop((ComIDataObject)e.Data, ref wp, (int)e.Effect);

            Point mousePosition = flpProgram.PointToClient(new Point(e.X, e.Y));
            int indexDest = flpProgram.Controls.Count;
            try
            {
                PictureBox dest = (PictureBox)flpProgram.GetChildAtPoint(mousePosition);
                indexDest = flpProgram.Controls.IndexOf(dest);
                if (indexDest != -1)
                    dest.BackgroundImage = null;
            }
            catch (Exception)
            {
                throw;
            }
            //second case
            if ((pbs.Margin.Left > 10) | (pbs.Margin.Top > 10))
            {
                flpProgram.Controls.SetChildIndex(pbs, indexDest);
                pbCurrentDraggedItem = null;
            }
            //first case
            else
            {
                pbJustDropped = new PictureBox();
                pbJustDropped.Image = pbs.Image;
                pbJustDropped.Name = pbs.Name;
                pbJustDropped.Size = new System.Drawing.Size(80, 80);
                pbJustDropped.Margin = instructionsPadding;
                pbJustDropped.Padding = new Padding(10);
                pbJustDropped.SizeMode = System.Windows.Forms.PictureBoxSizeMode.StretchImage;
                pbJustDropped.TabStop = false;
                pbJustDropped.MouseDown += new System.Windows.Forms.MouseEventHandler(prg_MouseDown);
                flpProgram.Controls.Add(pbJustDropped);
                flpProgram.Controls.SetChildIndex(pbJustDropped, indexDest);
            }
        }

        /// <summary>
        /// Start drag and drop of a dragged picturebox inside exec panel, shows drag icon
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void prg_MouseDown(object sender, MouseEventArgs e)
        {
            pbCurrentDraggedItem = sender as PictureBox;
            pbCurrentDraggedItem.BorderStyle = BorderStyle.Fixed3D;

            Bitmap bmp = (Bitmap)pbCurrentDraggedItem.Image;
            DataObject data = new DataObject(new DragDropLib.DataObject());
            ShDragImage shdi = new ShDragImage();
            Win32Size size;
            size.cx = bmp.Width;
            size.cy = bmp.Height;
            shdi.sizeDragImage = size;
            Point p = e.Location;
            Win32Point wpt;
            wpt.x = p.X;
            wpt.y = p.Y;
            shdi.ptOffset = wpt;
            shdi.hbmpDragImage = bmp.GetHbitmap();
            shdi.crColorKey = Color.Magenta.ToArgb();
            IDragSourceHelper sourceHelper = (IDragSourceHelper)new DragDropHelper();
            sourceHelper.InitializeFromBitmap(ref shdi, data);
            DoDragDrop(data, DragDropEffects.Copy);
        }

        /// <summary>
        /// Drag an instruction away from exec panel. Delete dragged instruction from alg.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void flpActions_DragDrop(object sender, DragEventArgs e)
        {
            e.Effect = DragDropEffects.Copy;
            Point p = Cursor.Position;
            Win32Point wp;
            wp.x = p.X;
            wp.y = p.Y;
            IDropTargetHelper dropHelper = (IDropTargetHelper)new DragDropHelper();
            dropHelper.Drop((ComIDataObject)e.Data, ref wp, (int)e.Effect);
            flpProgram.Controls.Remove(pbCurrentDraggedItem);
        }

        ///Change the algorithm panel layout, and corresponding placement marks.
        private void downUpToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (flpProgram.FlowDirection != (FlowDirection.TopDown | FlowDirection.BottomUp))
            {
                instructionsPadding = new Padding(80, 0, 0, 0);
                foreach (PictureBox p in flpProgram.Controls)
                {
                    p.Margin = instructionsPadding;
                }
            }
            bmpUp = bmpDragUp;
            bmpDown = bmpDragDown;
            flpProgram.FlowDirection = FlowDirection.BottomUp;
        }

        private void upDownToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (flpProgram.FlowDirection != (FlowDirection.BottomUp | FlowDirection.TopDown))
            {
                instructionsPadding = new Padding(80, 0, 0, 0);
                foreach (PictureBox p in flpProgram.Controls)
                {
                    p.Margin = instructionsPadding;
                }
            }
            bmpUp = bmpDragDown;
            bmpDown = bmpDragUp;
            flpProgram.FlowDirection = FlowDirection.TopDown;
        }

        private void leftRightToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (flpProgram.FlowDirection != (FlowDirection.RightToLeft | FlowDirection.LeftToRight))
            {
                instructionsPadding = new Padding(0, 50, 0, 0);
                foreach (PictureBox p in flpProgram.Controls)
                {
                    p.Margin = instructionsPadding;
                }
            }
            bmpUp = bmpDragRight;
            bmpDown = bmpDragLeft;
            flpProgram.FlowDirection = FlowDirection.LeftToRight;
        }

        private void rightLeftToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (flpProgram.FlowDirection != (FlowDirection.RightToLeft | FlowDirection.LeftToRight))
            {
                instructionsPadding = new Padding(0, 50, 0, 0);
                foreach (PictureBox p in flpProgram.Controls)
                {
                    p.Margin = instructionsPadding;
                }
            }
            bmpUp = bmpDragLeft;
            bmpDown = bmpDragRight;
            flpProgram.FlowDirection = FlowDirection.RightToLeft;
        }


        /// <summary>
        /// Show drag icon when an instruction is dragged into a panel's bounds.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void On_DragEnter(object sender, DragEventArgs e)
        {
            e.Effect = DragDropEffects.Copy;
            Point p = Cursor.Position;
            Win32Point wp;
            wp.x = p.X;
            wp.y = p.Y;
            IDropTargetHelper dropHelper = (IDropTargetHelper)new DragDropHelper();
            dropHelper.DragEnter(IntPtr.Zero, (ComIDataObject)e.Data, ref wp, (int)e.Effect);
        }

        /// <summary>
        /// Show drag icon when an instruction is dragged over a panel's bounds.
        /// When dragging occurs in a program panel, show also
        /// a mark where the instruction shall be placed.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void On_DragOver(object sender, DragEventArgs e)
        {
            //show drag effects
            e.Effect = DragDropEffects.Copy;
            Point p = Cursor.Position;
            Win32Point wp;
            wp.x = p.X;
            wp.y = p.Y;
            IDropTargetHelper dropHelper = (IDropTargetHelper)new DragDropHelper();
            dropHelper.DragOver(ref wp, (int)e.Effect);

            //show placement mark
            Point mousePosition = flpProgram.PointToClient(new Point(e.X, e.Y));
            try
            {
                PictureBox dest = (PictureBox)flpProgram.GetChildAtPoint(mousePosition);
                int indexDest = flpProgram.Controls.IndexOf(dest);
                int indexSource = flpProgram.Controls.IndexOf(pbCurrentDraggedItem);

                //drag up
                if ((indexDest > indexSource) && (indexSource != -1))
                    bmpDrag = bmpUp;
                //drag over self
                else if (indexDest == indexSource)
                    bmpDrag = null;
                //drag down
                else
                    bmpDrag = bmpDown;

                if (indexDest != -1)
                {
                    if (lastDraggedOverPb == -1)
                    {
                        //dest.BackColor = Color.SkyBlue;
                        dest.BackgroundImage = bmpDrag;
                    }
                    else
                    {
                        if (lastDraggedOverPb != indexDest)
                        {
                            flpProgram.Controls[lastDraggedOverPb].BackgroundImage = null;
                            dest.BackgroundImage = bmpDrag;
                        }
                    }
                }
                else if (indexDest == -1)
                {
                    if (lastDraggedOverPb != -1)
                        flpProgram.Controls[lastDraggedOverPb].BackgroundImage = null;
                }
                lastDraggedOverPb = indexDest;
            }
            catch (System.ArgumentOutOfRangeException)
            {
                //Console.WriteLine("No pb found");
            }
        }

        private void On_DragLeave(object sender, EventArgs e)
        {
            IDropTargetHelper dropHelper = (IDropTargetHelper)new DragDropHelper();
            dropHelper.DragLeave();
        }

        private void OnApplicationExit(object sender, EventArgs e)
        {
            gameControllerThreadRunning = false;
            while ((executeAlgorithm != null && executeAlgorithm.IsAlive) || 
                (gameControllerThread != null && gameControllerThread.IsAlive))
            {
                Thread.Sleep(200);
            }
            releaseJoystick();
            
            if (nxtBrick.IsConnected)
            {
                nxtBrick.Disconnect();
            }
        }

        #endregion

        private void driverToolStripMenuItem_Click(object sender, EventArgs e)
        {
            setDriverMode(ROBOTMODE.DRIVER);
            Properties.Settings.Default.Save();
        }

        private void alligatorToolStripMenuItem_Click(object sender, EventArgs e)
        {
            setDriverMode(ROBOTMODE.ALLIGATOR);
            Properties.Settings.Default.Save();
        }

        private void robohumanToolStripMenuItem_Click(object sender, EventArgs e)
        {
            setDriverMode(ROBOTMODE.ROBOHUMAN);
            Properties.Settings.Default.Save();
        }

        private void setDriverMode(ROBOTMODE mode)
        {
            robotMode = mode;
            switch (mode)
            {
                case ROBOTMODE.DRIVER:
                    driverModeLabel.Text = driverToolStripMenuItem.Text;
                    nxtMotor1.Flip = false;
                    nxtMotor2.Flip = false;
                    nxtMotor3.Flip = false;
                    motorTurnDelay = Convert.ToInt32(tsTxtMotorTurn.Text);
                    motorSpeed = 90;
                    break;
                case ROBOTMODE.ALLIGATOR:
                    driverModeLabel.Text = alligatorToolStripMenuItem.Text;
                    nxtMotor1.Flip = true;
                    nxtMotor2.Flip = true;
                    nxtMotor3.Flip = true;
                    motorTurnDelay = Convert.ToInt32(tsTxtMotorTurn.Text) * 2;
                    motorSpeed = 75;
                    break;
                case ROBOTMODE.ROBOHUMAN:
                    driverModeLabel.Text = robohumanToolStripMenuItem.Text;
                    nxtMotor1.Flip = false;
                    nxtMotor2.Flip = false;
                    nxtMotor3.Flip = false;
                    motorTurnDelay = Convert.ToInt32(tsTxtMotorTurn.Text);
                    motorSpeed = 35;
                    break;
            }
            Properties.Settings.Default.DriverMode = driverModeLabel.Text;
        }

        private void nxtBrick_Connected(object sender, EventArgs e)
        {
            connectedLabel.Text = "Yhdistetty porttiin " + nxtBrick.COMPortName;
            setJoystick();
        }

        private void nxtBrick_Disconnected(object sender, EventArgs e)
        {
            connectedLabel.Text = "Ei yhteyttä robottiin";
            //gameControllerThreadRunning = false;
        }

        private void tsTxtComPort_TextChanged(object sender, EventArgs e)
        {
            if (nxtBrick.IsConnected)
            {
                nxtBrick.Disconnect();
            }
            Properties.Settings.Default.ComPort = tsTxtComPort.Text;
            Properties.Settings.Default.Save();
        }

        private void tsTxtMotorTurn_TextChanged(object sender, EventArgs e)
        {
            try
            {
                motorTurnDelay = Convert.ToInt32(tsTxtMotorTurn.Text);
                if (robotMode == ROBOTMODE.ALLIGATOR)
                {
                    motorTurnDelay = motorTurnDelay * 2;
                }
                Properties.Settings.Default.TurnDelay = motorTurnDelay;
                Properties.Settings.Default.Save();
            }
            catch (Exception)
            {
                tsTxtMotorTurn.Text = Properties.Settings.Default.TurnDelay.ToString();
            }
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            setJoystick();
        }

        private void setJoystick()
        {
            releaseJoystick();
            // make sure that DirectInput has been initialized
            DirectInput dinput = new DirectInput();

            // search for devices
            foreach (DeviceInstance device in dinput.GetDevices(DeviceClass.GameController, DeviceEnumerationFlags.AttachedOnly))
            {
                // create the device
                try
                {
                    joystick = new Joystick(dinput, device.InstanceGuid);
                    joystick.SetCooperativeLevel(this, CooperativeLevel.Exclusive | CooperativeLevel.Foreground);
                    break;
                }
                catch (DirectInputException)
                {
                }
            }

            if (joystick == null)
            {
                Console.Out.WriteLine("There are no joysticks attached to the system.");
                return;
            }

            foreach (DeviceObjectInstance deviceObject in joystick.GetObjects())
            {
                if ((deviceObject.ObjectType & ObjectDeviceType.Axis) != 0)
                    joystick.GetObjectPropertiesById((int)deviceObject.ObjectType).SetRange(-1000, 1000);
            }

            // acquire the device
            joystick.Acquire();
            setInfoText("Joystik liitetty");

            prevJoystickX = 0;
            prevJoystickY = 0;
            prevJoystickAction = 0;
            if (nxtBrick.IsConnected)
            {
                coastMotor(1);
                coastMotor(2);
                coastMotor(3);
                //start polling thread
                gameControllerThread = new Thread(gameControllerPoller);
                gameControllerThread.Start();
            }
            else
            {
                gameControllerThread = new Thread(gameControllerPoller);
                gameControllerThread.Start();
            }
        }
        

        private void connectToolStripMenuItem_Click(object sender, EventArgs e)
        {
            setJoystick();
        }

        private void playNXTSound()
        {
        }

        private void tsTxtJoystickSensitivity_TextChanged(object sender, EventArgs e)
        {
            try
            {
                joystickSensitivity = Convert.ToInt32(tsTxtJoystickSensitivity.Text);
                Properties.Settings.Default.JoystickSensitivity = joystickSensitivity;
                Properties.Settings.Default.Save();
            }
            catch (Exception)
            {
                tsTxtJoystickSensitivity.Text = Properties.Settings.Default.JoystickSensitivity.ToString();
            }
        }

    }
}
